#include "RobotManager.h"
#include "TcpServer.h"
#include "RobotClient.h"
#include "DisconnectLogger.h"
#include <QDateTime>
#include <QCoreApplication>
#include <QDir>
#include <QFile>
#include <QJsonDocument>
#include <QJsonObject>
#include <QUuid> 
#include <QTextStream>
#include <QtMath>

RobotManager::RobotManager(QObject *parent)
    : QObject(parent), m_controlLog(nullptr), m_heartbeatDebugLog(nullptr), m_heartbeatDebugStream(nullptr), m_controlInterface(nullptr)
{
    // 初始化数据库管理器
    m_dbManager = new DatabaseManager(this);
    initializeDatabase();
     
    // 初始化TCP服务器
    m_tcpServer = new TcpServer(this);
    
    // 初始化内存状态缓存（关键优化：内存优先架构）
    m_memoryCache = new RobotMemoryCache(this);
    
    // 初始化QML控制接口
    m_controlInterface = new QMLControlInterface(this);
    
    // 初始化心跳调试日志
    initializeHeartbeatDebugLog();
    
    // 连接服务器信号
    connect(m_tcpServer, &TcpServer::serverStarted, this, &RobotManager::onServerStarted);
    connect(m_tcpServer, &TcpServer::serverStopped, this, &RobotManager::onServerStopped);
    connect(m_tcpServer, &TcpServer::clientConnected, this, &RobotManager::onClientConnected);
    connect(m_tcpServer, &TcpServer::clientDisconnected, this, &RobotManager::onClientDisconnected);
    connect(m_tcpServer, &TcpServer::serverError, this, &RobotManager::onServerError);
    connect(m_tcpServer, &TcpServer::robotRegistered, this, &RobotManager::onRobotRegistered);
    connect(m_tcpServer, &TcpServer::robotDataReceived, this, &RobotManager::onRobotDataReceived);
    connect(m_tcpServer, &TcpServer::robotHeartbeatReceived, this, &RobotManager::onRobotHeartbeatReceived);
    connect(m_tcpServer, &TcpServer::robotDisconnected, this, &RobotManager::onRobotDisconnected);
    connect(m_tcpServer, &TcpServer::jsonParseError, this, [this](const QString &clientIp, const QString &rawData, const QString &errorMsg) {
        // 记录JSON解析错误到日志文件
        logJsonParseError(clientIp, rawData, errorMsg);
    });
    
    // 初始化离线检查定时器
    m_offlineTimer = new QTimer(this);
    m_offlineTimer->setInterval(OFFLINE_CHECK_INTERVAL);
    connect(m_offlineTimer, &QTimer::timeout, this, &RobotManager::updateOfflineRobots);
    
    // 初始化时间同步定时器
    m_timeSyncTimer = new QTimer(this);
    m_timeSyncTimer->setInterval(TIME_SYNC_INTERVAL);
    connect(m_timeSyncTimer, &QTimer::timeout, this, &RobotManager::onTimeSyncTimeout);
    
    // 初始化数据归档定时器（关键优化：批量数据库写入）
    m_archiveTimer = new QTimer(this);
    m_archiveTimer->setInterval(ARCHIVE_INTERVAL);
    connect(m_archiveTimer, &QTimer::timeout, this, &RobotManager::onArchiveTimeout);
    
    // 连接内存缓存信号
    connect(m_memoryCache, &RobotMemoryCache::robotStatusUpdated, 
            this, &RobotManager::onRobotStatusUpdated);
    connect(m_memoryCache, &RobotMemoryCache::robotConnectionChanged, 
            this, &RobotManager::onRobotConnectionChanged);
    
    // 初始化控制命令日志文件
    QString logDir = QCoreApplication::applicationDirPath() + "/logs";
    QDir dir(logDir);
    if (!dir.exists()) {
        dir.mkpath(logDir);
    }
    
    QString logFileName = logDir + "/control_commands.log";
    m_controlLog = new QFile(logFileName, this);
}

RobotManager::~RobotManager()
{
    stopServer();
    
    // 关闭日志文件
    if (m_controlLog && m_controlLog->isOpen()) {
        m_controlLog->close();
    }
    
    // 关闭心跳调试日志文件
    if (m_heartbeatDebugStream) {
        delete m_heartbeatDebugStream;
    }
    if (m_heartbeatDebugLog && m_heartbeatDebugLog->isOpen()) {
        m_heartbeatDebugLog->close();
    }
    delete m_heartbeatDebugLog;
}

bool RobotManager::startServer(int port)
{
    bool success = m_tcpServer->startServer(port);
    if (success) {
        m_offlineTimer->start();
        m_timeSyncTimer->start();
        m_archiveTimer->start(); // 启动数据归档定时器
    }
    return success;
}

void RobotManager::stopServer()
{
    m_offlineTimer->stop();
    m_timeSyncTimer->stop();
    m_archiveTimer->stop(); // 停止数据归档定时器
    
    // 归档所有数据到数据库（关键优化：确保数据不丢失）
    archiveRobotData();
    
    m_tcpServer->stopServer();
}

bool RobotManager::isServerRunning() const
{
    return m_tcpServer->isRunning();
}

QString RobotManager::getServerAddress() const
{
    return m_tcpServer->getServerAddress();
}

int RobotManager::getServerPort() const
{
    return m_tcpServer->getServerPort();
}

int RobotManager::getConnectedRobotsCount() const
{
    return m_tcpServer->getConnectedClientsCount();
}

QVariantList RobotManager::getAllRobots()
{
    // 【关键优化】内存优先：优先从内存获取实时状态
    QVariantList memoryRobots = m_memoryCache->getAllOnlineRobots();
    if (!memoryRobots.isEmpty()) {
        return memoryRobots;
    }
    
    // 内存中没有数据时，回退到数据库
    return m_dbManager->getAllRobots();
}

QVariantMap RobotManager::getRobotInfo(const QString &sn)
{
    // 【关键优化】内存优先：优先从内存获取实时状态
    QVariantMap memoryInfo = m_memoryCache->getRobotStatus(sn);
    if (!memoryInfo.isEmpty()) {
        return memoryInfo;
    }
    
    // 内存中没有数据时，回退到数据库
    return m_dbManager->getRobotInfo(sn);
}

QVariantList RobotManager::getRobotData(const QString &sn, int limit)
{
    // 【关键优化】内存优先：优先从内存获取最新数据
    QVariantMap currentData = m_memoryCache->getRobotStatus(sn);
    if (!currentData.isEmpty()) {
        // 返回当前状态作为最新数据点
        QVariantList result;
        result.append(currentData);
        return result;
    }
    
    // 内存中没有数据时，回退到数据库历史数据
    return m_dbManager->getRobotData(sn, limit);
}

bool RobotManager::sendControlCommand(const QString &sn, const QVariantMap &command)
{
    RobotClient *client = findClientBySN(sn);
    if (!client) {
        qDebug() << QString("无法发送控制命令：未找到机器人 %1").arg(sn);
        return false;
    }
    
    // 检查客户端是否已注册且连接正常
    bool isConnected = client->isConnected();
    bool isRegistered = client->isRegistered();
    qDebug() << QString("检查机器人状态：SN=%1, 已连接=%2, 已注册=%3").arg(sn).arg(isConnected).arg(isRegistered);
    
    if (!isConnected || !isRegistered) {
        qDebug() << QString("无法发送控制命令：机器人 %1 未连接或未注册").arg(sn);
        return false;
    }
    
    // 将QVariantMap转换为QJsonObject
    QJsonObject jsonCommand;
    for (auto it = command.begin(); it != command.end(); ++it) {
        jsonCommand[it.key()] = QJsonValue::fromVariant(it.value());
    }
    
    // 发送命令
    client->sendMessage(jsonCommand);
    
    // 记录日志
    logControlCommand(sn, jsonCommand);
    
    qDebug() << QString("已发送控制命令到机器人 %1: %2").arg(sn).arg(QString(QJsonDocument(jsonCommand).toJson(QJsonDocument::Compact)));
    return true;
}

bool RobotManager::sendTimeSyncCommand(const QString &sn)
{
    RobotClient *client = findClientBySN(sn);
    if (!client) {
        qDebug() << QString("无法发送时间同步命令：未找到机器人 %1").arg(sn);
        return false;
    }
    
    // 检查客户端是否已注册且连接正常
    if (!client->isConnected() || !client->isRegistered()) {
        qDebug() << QString("无法发送时间同步命令：机器人 %1 未连接或未注册").arg(sn);
        return false;
    }
    
    // 生成唯一同步标识符
    QString syncId = QUuid::createUuid().toString(QUuid::Id128);
    
    // 创建时间同步消息
    QJsonObject timeSyncMessage;
    timeSyncMessage["type"] = "time_sync";
    timeSyncMessage["timestamp"] = QDateTime::currentMSecsSinceEpoch();
    timeSyncMessage["sync_id"] = syncId;
    
    // 发送时间同步消息
    client->sendMessage(timeSyncMessage);
    
    // 记录日志
    logTimeSyncCommand(sn, syncId, true);
    
    qDebug() << QString("已发送时间同步命令到机器人 %1: %2").arg(sn).arg(QString(QJsonDocument(timeSyncMessage).toJson(QJsonDocument::Compact)));
    return true;
}

QVariantList RobotManager::generateRawCommands(const QString &direction)
{
    QVariantList rawCommands;
    
    qDebug() << "生成方向控制指令：" << direction;
    
    // 【关键修改】根据用户需求：只生成单条指令
    if (direction == "forward") {
        // 只发送行进控制指令（D122 = 25）
        QVariantMap moveCommand;
        moveCommand["register"] = "D122";
        moveCommand["address"] = 122;
        moveCommand["functionCode"] = 6;
        moveCommand["value"] = -25;  // 前进速度
        rawCommands.append(moveCommand);
        
        qDebug() << "生成前进指令：仅D122=" << 25;
        
    } else if (direction == "stop") {
        // 停止时同时置零行进控制和转向控制（D122 = 0，D121 = 0）
        
        // 停止行进控制（D122 = 0）
        QVariantMap stopMoveCommand;
        stopMoveCommand["register"] = "D122";
        stopMoveCommand["address"] = 122;
        stopMoveCommand["functionCode"] = 6;
        stopMoveCommand["value"] = 0;   // 停止行进
        rawCommands.append(stopMoveCommand);
        
        // 停止转向控制（D121 = 0）
        QVariantMap stopTurnCommand;
        stopTurnCommand["register"] = "D121";
        stopTurnCommand["address"] = 121;
        stopTurnCommand["functionCode"] = 6;
        stopTurnCommand["value"] = 0;   // 停止转向
        rawCommands.append(stopTurnCommand);
        
        qDebug() << "生成停止指令：D122=" << 0 << ", D121=" << 0;
        
    } else if (direction == "backward") {
        // 只发送行进控制指令（D122 = -25）
        QVariantMap moveCommand;
        moveCommand["register"] = "D122";
        moveCommand["address"] = 122;
        moveCommand["functionCode"] = 6;
        moveCommand["value"] = 25; // 后退速度
        rawCommands.append(moveCommand);
        
        qDebug() << "生成后退指令：仅D122=" << -25;
        
    } else if (direction == "left") {
        // 只发送转向控制指令（D121 = -12）
        QVariantMap turnCommand;
        turnCommand["register"] = "D121";
        turnCommand["address"] = 121;
        turnCommand["functionCode"] = 6;
        turnCommand["value"] = 12; // 左转角度
        rawCommands.append(turnCommand);
        
        qDebug() << "生成左转指令：仅D121=" << -12;
        
    } else if (direction == "right") {
        // 只发送转向控制指令（D121 = 12）
        QVariantMap turnCommand;
        turnCommand["register"] = "D121";
        turnCommand["address"] = 121;
        turnCommand["functionCode"] = 6;
        turnCommand["value"] = -12;  // 右转角度
        rawCommands.append(turnCommand);
        
        qDebug() << "生成右转指令：仅D121=" << 12;
    }
    
    qDebug() << "生成的指令数量：" << rawCommands.size();
    return rawCommands;
}

void RobotManager::onServerStarted(const QString &address, int port)
{
    qDebug() << QString("机器人管理服务器已启动: %1:%2").arg(address).arg(port);
    emit serverRunningChanged();
    emit serverAddressChanged();
    emit serverPortChanged();
}

void RobotManager::onServerStopped()
{
    qDebug() << "机器人管理服务器已停止";
    emit serverRunningChanged();
    emit serverAddressChanged();
    emit serverPortChanged();
}

void RobotManager::onClientConnected(const QString &address, int port)
{
    qDebug() << QString("新客户端连接到机器人管理器: %1:%2").arg(address).arg(port);
    emit connectedRobotsCountChanged();
}

void RobotManager::onClientDisconnected(const QString &address)
{
    qDebug() << QString("客户端从机器人管理器断开: %1").arg(address);
    emit connectedRobotsCountChanged();
}

void RobotManager::onRobotRegistered(const QString &sn)
{
    RobotClient *client = findClientBySN(sn);
    if (client) {
        QString ip = client->getClientAddress();
        int port = client->getClientPort();
        
        // 检查IP地址有效性
        if (ip.isEmpty() || ip == ":0") {
            qDebug() << QString("注册失败：无效的IP地址 SN=%1, IP=%2").arg(sn).arg(ip);
            // 发送注册失败响应给客户端
            QJsonObject response;
            response["type"] = "register_ack";
            response["status"] = "failed";
            response["message"] = "无效的IP地址";
            // 注意：这里不能直接发送，因为客户端可能已经断开连接
            return;
        }
        
        // 【关键优化】内存优先：先注册到内存缓存
    m_memoryCache->updateRobotConnection(sn, true, QDateTime::currentDateTime());
        
        // 异步注册到数据库（不阻塞TCP响应）
        m_dbManager->registerRobot(sn, ip, port);
        
        // 重置机器人状态
        m_dbManager->resetRobotStatus(sn);
        
        qDebug() << QString("机器人已注册: SN=%1, IP=%2:%3").arg(sn).arg(ip).arg(port);
        
        // 注册完成后延迟执行时间同步，避免在刚注册时发送命令
        QTimer::singleShot(3000, [this, sn, client]() {
            // 再次检查客户端是否仍然连接和注册
            if (client->isConnected() && client->isRegistered()) {
                if (sendTimeSyncCommand(sn)) {
                    qDebug() << QString("时间同步命令发送成功: %1").arg(sn);
                } else {
                    qDebug() << QString("时间同步命令发送失败: %1").arg(sn);
                }
            } else {
                qDebug() << QString("客户端状态已改变，取消时间同步: %1").arg(sn);
            }
        });
        
        emit robotRegistered(sn);
    }
}

void RobotManager::onRobotDataReceived(const QString &sn, const QJsonObject &data)
{
    // 【关键优化】内存优先：先更新内存状态，不直接操作数据库
    m_memoryCache->updateRobotStatus(sn, data.toVariantMap());
    
    // 更新机器人心跳时间（仅更新内存，不操作数据库）
    m_memoryCache->updateRobotConnection(sn, true, QDateTime::currentDateTime());
    
    // 更新机器人的连接信息（IP和端口可能发生变化）
    RobotClient* client = findClientBySN(sn);
    if (client) {
        QString ip = client->getClientAddress();
        int port = client->getClientPort();
        // 检查IP地址是否有效再更新
        if (!ip.isEmpty() && ip != ":0") {
            // 更新内存中的连接信息
        m_memoryCache->updateRobotConnection(sn, true, QDateTime::currentDateTime());
        } else {
            qDebug() << "跳过更新机器人连接信息，无效IP地址: " << ip;
        }
    }

    // 检查是否包含位置信息，如果有则发送实时定位信号s
    if (data.contains("longitude") && data.contains("latitude")) {
        // 处理新的定位格式：{"longitude":109.166854,"latitude":23.758725}
        double longitude = data.value("longitude").toDouble();
        double latitude = data.value("latitude").toDouble();

        qDebug() << "RobotManager::onRobotDataReceived() 检测到新格式定位信息，SN:" << sn
                 << "经度:" << longitude << "纬度:" << latitude;

        // 发送实时定位信号，跳过数据库直接发送给QML界面
        emit robotLocationUpdated(sn, longitude, latitude);

        // 更新路径跟踪位置
        updateRobotPositionForTracking(sn, longitude, latitude);

    } else if (data.contains("position")) {
        // 【兼容旧格式】处理旧的{"position":{"x":109.166854,"y":23.758725}}格式
        QJsonObject pos = data.value("position").toObject();
        if (pos.contains("x") && pos.contains("y")) {
            double longitude = pos.value("x").toDouble();
            double latitude = pos.value("y").toDouble();

            qDebug() << "RobotManager::onRobotDataReceived() 检测到旧格式定位信息，SN:" << sn
                     << "经度:" << longitude << "纬度:" << latitude;

            // 发送实时定位信号，跳过数据库直接发送给QML界面
            emit robotLocationUpdated(sn, longitude, latitude);

            // 更新路径跟踪位置
            updateRobotPositionForTracking(sn, longitude, latitude);
        }
    }
    
    // 检查是否是时间同步响应
    if (data.contains("type") && data.value("type").toString() == "time_sync_ack") {
        QString syncId = data.value("sync_id").toString();
        QString status = data.value("status").toString();
        
        // 记录时间同步响应日志
        logTimeSyncCommand(sn, syncId, status == "success");
        
        // 发送时间同步响应数据到QML
        QVariantMap timeSyncData;
        timeSyncData["type"] = "time_sync_ack";
        timeSyncData["sync_id"] = syncId;
        timeSyncData["status"] = status;
        timeSyncData["message"] = data.value("message").toString();
        timeSyncData["timestamp"] = data.value("timestamp").toVariant().toLongLong();
        emit robotDataReceived(sn, timeSyncData);
        return;
    }
    
    // 统一处理RTSP地址响应（兼容新旧格式）
    if (data.contains("type") && (data.value("type").toString() == "rtsp_urls" || data.value("type").toString() == "rtsp_urls_response")) {
        // 创建包含RTSP地址的数据映射
        QVariantMap rtspData;
        rtspData["type"] = data.value("type").toString();
        
        // 处理旧格式的RTSP地址响应
        if (data.value("type").toString() == "rtsp_urls") {
            // 提取前后摄像头地址
            if (data.contains("front_camera")) {
                rtspData["front_camera"] = data.value("front_camera").toString();
            }
            if (data.contains("rear_camera")) {
                rtspData["rear_camera"] = data.value("rear_camera").toString();
            }
        }
        // 处理新格式的RTSP地址响应
        else if (data.value("type").toString() == "rtsp_urls_response") {
            // 提取RTSP URL数组
            if (data.contains("rtsp_urls")) {
                rtspData["rtsp_urls"] = data.value("rtsp_urls").toArray().toVariantList();
                // 输出RTSP URL数组内容
                QJsonArray rtspUrls = data.value("rtsp_urls").toArray();
            }
        }
        
        // 发送RTSP地址数据到QML
        emit robotDataReceived(sn, rtspData);
        return;
    }
    
    // 解析数据到QVariantMap
    QVariantMap dataMap;
    if (data.contains("position")) {
        QJsonObject pos = data.value("position").toObject();
        double posX = pos.value("x").toDouble();
        double posY = pos.value("y").toDouble();
        double posZ = pos.value("z").toDouble();
        
        dataMap["pos_x"] = posX;
        dataMap["pos_y"] = posY;
        dataMap["pos_z"] = posZ;
    }
    
    if (data.contains("temperature")) {
        dataMap["temperature"] = data.value("temperature").toDouble();
    }
    
    if (data.contains("battery")) {
        dataMap["battery"] = data.value("battery").toDouble();
    }
    
    if (data.contains("status")) {
        dataMap["status"] = data.value("status").toString();
    }
    
    // 解析PLC数据
    if (data.contains("plc")) {
        QJsonObject plcData = data.value("plc").toObject();
        dataMap["plc"] = plcData.toVariantMap();
    }
    
    // 发送数据更新信号（直接从内存获取最新状态）
    QVariantMap currentStatus = m_memoryCache->getRobotStatus(sn);
    emit robotDataReceived(sn, currentStatus);
    
    // 【关键优化】内存优先：不再直接存储到数据库，由归档定时器批量处理
    // m_dbManager->insertFullRobotData(sn, dataMap);
}

void RobotManager::onRobotDisconnected(const QString &sn)
{
    // 【关键优化】内存优先：先更新内存状态
    m_memoryCache->updateRobotConnection(sn, false, QDateTime::currentDateTime());
    
    // 归档机器人数据到数据库（关键优化：确保数据不丢失）
    QVariantMap robotData = m_memoryCache->getRobotStatus(sn);
    if (!robotData.isEmpty()) {
        // 将最终状态写入数据库
        m_dbManager->insertFullRobotData(sn, robotData);
        qDebug() << QString("机器人 %1 断开连接，数据已归档").arg(sn);
    }
    
    // 将数据库中机器人状态设为离线
    if (m_dbManager->updateRobotStatus(sn, false)) {
        qDebug() << QString("机器人 %1 已标记为离线状态").arg(sn);
    } else {
        qDebug() << QString("更新机器人 %1 离线状态失败").arg(sn);
    }
    
    // 发出机器人断开连接信号
    emit robotDisconnected(sn);
}

void RobotManager::onRobotHeartbeatReceived(const QString &sn, const QJsonObject &data)
{
    // 【关键优化】心跳隔离：仅更新内存状态，不操作数据库
    
    // 更新内存中的心跳时间
    m_memoryCache->updateRobotConnection(sn, true, QDateTime::currentDateTime());
    
    qDebug() << QString("机器人 %1 心跳已更新到内存缓存").arg(sn);
}

void RobotManager::onServerError(const QString &error)
{
    qDebug() << "机器人管理器服务器错误：" << error;
    emit serverError(error);
}

void RobotManager::onArchiveTimeout()
{
    // 【关键优化】定时归档数据到数据库
    archiveRobotData();
}

void RobotManager::archiveRobotData()
{
    // 获取需要归档的机器人数据
    QVariantList robotsToArchive = m_memoryCache->getRobotsForArchiving();
    
    if (robotsToArchive.isEmpty()) {
        return; // 没有需要归档的数据
    }
    
    qDebug() << QString("开始归档 %1 个机器人的数据到数据库").arg(robotsToArchive.size());
    
    // 批量归档数据
    for (const QVariant &robotVariant : robotsToArchive) {
        QVariantMap robotInfo = robotVariant.toMap();
        QString sn = robotInfo.value("sn").toString();
        QVariantMap robotData = robotInfo.value("data").toMap();
        
        if (!robotData.isEmpty()) {
            // 异步归档到数据库
            m_dbManager->insertFullRobotData(sn, robotData);
            
            // 更新内存中的归档时间
            m_memoryCache->markAsArchived(sn);
        }
    }
    
    qDebug() << "数据归档完成";
}

void RobotManager::updateOfflineRobots()
{
    // 【关键优化】内存优先：使用内存状态检测超时机器人
    QVariantList allRobots = m_memoryCache->getAllOnlineRobots();
    QVariantList timeoutRobots;
    
    // 手动检测超时机器人（45秒无心跳）
    qint64 currentTime = QDateTime::currentMSecsSinceEpoch();
    qint64 timeoutThreshold = 45 * 1000; // 45秒
    
    for (const QVariant &robotVariant : allRobots) {
        QVariantMap robotInfo = robotVariant.toMap();
        QString sn = robotInfo.value("sn").toString();
        QDateTime lastPing = robotInfo.value("last_ping").toDateTime();
        
        if (lastPing.isValid() && (currentTime - lastPing.toMSecsSinceEpoch()) > timeoutThreshold) {
            timeoutRobots.append(robotInfo);
        }
    }
    
    if (!timeoutRobots.isEmpty()) {
        qDebug() << QString("发现 %1 个超时机器人，即将关闭连接").arg(timeoutRobots.size());
        
        for (const QVariant &robotVariant : timeoutRobots) {
            QVariantMap robotInfo = robotVariant.toMap();
            QString sn = robotInfo.value("sn").toString();
            QString lastPing = robotInfo.value("last_ping").toString();
            qint64 lastPingTime = QDateTime::fromString(lastPing, "yyyy-MM-dd hh:mm:ss").toMSecsSinceEpoch();
            qint64 currentTime = QDateTime::currentMSecsSinceEpoch();
            qint64 timeoutDuration = currentTime - lastPingTime;
            
            qDebug() << QString("机器人 %1 超时，最后心跳: %2").arg(sn).arg(lastPing);
            
            // 记录数据库超时断开日志
            DisconnectLogger::instance()->logDatabaseTimeout(
                sn,
                lastPing,
                timeoutDuration
            );
            
            // 主动断开TCP连接
            if (m_tcpServer->disconnectRobotBySN(sn)) {
                qDebug() << QString("已断开机器人 %1 的TCP连接").arg(sn);
            }
            
            // 更新数据库状态为离线
            if (m_dbManager->updateRobotStatus(sn, false)) {
                qDebug() << QString("机器人 %1 已标记为离线状态（超时）").arg(sn);
                emit robotDisconnected(sn);
            }
        }
    } else {
        qDebug() << "没有发现超时机器人";
    }
}

void RobotManager::onTimeSyncTimeout()
{
    qDebug() << "执行定时时间同步";
    
    // 获取所有已连接的机器人
    QList<RobotClient*> clients = m_tcpServer->getAllClients();
    
    // 为每个已连接的机器人发送时间同步命令
    for (RobotClient* client : clients) {
        if (client->isConnected() && client->isRegistered()) {
            QString sn = client->getRobotSN();
            sendTimeSyncCommand(sn);
        }
    }
}

void RobotManager::initializeDatabase()
{
    if (m_dbManager->connectToDB()) {
        m_dbManager->initializeUserTable();
        m_dbManager->initializeRobotTables();
        qDebug() << "RobotManager: 数据库初始化成功";
    } else {
        qDebug() << "RobotManager: 数据库连接失败，机器人数据存储功能将不可用";
        // 不调用qFatal，允许TCP服务器继续运行
    }
}

void RobotManager::logJsonParseError(const QString &clientIp, const QString &rawData, const QString &errorMsg)
{
    // 创建日志文件目录
    QString logDir = QCoreApplication::applicationDirPath() + "/logs";
    QDir dir(logDir);
    if (!dir.exists()) {
        dir.mkpath(logDir);
    }
    
    // 创建日志文件名（按日期分组）
    QString logFileName = logDir + "/json_errors_" + QDate::currentDate().toString("yyyyMMdd") + ".log";
    
    // 打开日志文件（追加模式）
    QFile logFile(logFileName);
    if (!logFile.open(QIODevice::WriteOnly | QIODevice::Append | QIODevice::Text)) {
        qDebug() << "无法打开JSON错误日志文件：" << logFileName;
        return;
    }
    
    // 创建日志条目，增加调试信息
    QJsonObject logEntry;
    logEntry["timestamp"] = QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss.zzz");
    logEntry["client_ip"] = clientIp;
    logEntry["error_message"] = errorMsg;
    logEntry["raw_data"] = rawData;
    logEntry["data_length"] = rawData.length();  // 添加数据长度信息
    logEntry["data_preview"] = rawData.left(100);  // 添加数据预览（前100字符）
    
    // 将日志条目转换为JSON字符串并写入文件
    QJsonDocument logDoc(logEntry);
    QByteArray logData = logDoc.toJson(QJsonDocument::Compact) + "\n";
    
    logFile.write(logData);
    logFile.close();
    
    qDebug() << QString("JSON解析错误已记录到日志文件：%1").arg(logFileName);
    qDebug() << QString("错误详情：IP=%1, 错误=%2, 数据长度=%3").arg(clientIp).arg(errorMsg).arg(rawData.length());
}

void RobotManager::logControlCommand(const QString &robotSN, const QJsonObject &command)
{
    // 确保日志文件已打开
    if (!m_controlLog->isOpen()) {
        if (!m_controlLog->open(QIODevice::WriteOnly | QIODevice::Append | QIODevice::Text)) {
            qDebug() << "无法打开控制命令日志文件：" << m_controlLog->fileName();
            return;
        }
    }
    
    // 创建日志条目
    QJsonObject logEntry;
    logEntry["timestamp"] = QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss.zzz");
    logEntry["robot_sn"] = robotSN;
    logEntry["command"] = command;
    
    // 将日志条目转换为JSON字符串并写入文件
    QJsonDocument logDoc(logEntry);
    QByteArray logData = logDoc.toJson(QJsonDocument::Compact) + "\n";
    
    m_controlLog->write(logData);
    m_controlLog->flush(); // 确保立即写入磁盘
    
    qDebug() << QString("控制命令已记录到日志文件：%1").arg(m_controlLog->fileName());
}

void RobotManager::logTimeSyncCommand(const QString &robotSN, const QString &syncId, bool success)
{
    // 创建日志文件目录
    QString logDir = QCoreApplication::applicationDirPath() + "/logs";
    QDir dir(logDir);
    if (!dir.exists()) {
        dir.mkpath(logDir);
    }
    
    // 创建时间同步日志文件名
    QString logFileName = logDir + "/time_sync.log";
    
    // 打开日志文件（追加模式）
    QFile logFile(logFileName);
    if (!logFile.open(QIODevice::WriteOnly | QIODevice::Append | QIODevice::Text)) {
        qDebug() << "无法打开时间同步日志文件：" << logFileName;
        return;
    }
    
    // 创建日志条目
    QJsonObject logEntry;
    logEntry["timestamp"] = QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss.zzz");
    logEntry["robot_sn"] = robotSN;
    logEntry["sync_id"] = syncId;
    logEntry["success"] = success;
    
    // 将日志条目转换为JSON字符串并写入文件
    QJsonDocument logDoc(logEntry);
    QByteArray logData = logDoc.toJson(QJsonDocument::Compact) + "\n";
    
    logFile.write(logData);
    logFile.close();
    
    qDebug() << QString("时间同步命令已记录到日志文件：%1").arg(logFileName);
}

void RobotManager::onRobotStatusUpdated(const QString &sn, const QVariantMap &status)
{
    // 内存状态更新处理（可用于监控和调试）
    qDebug() << QString("机器人 %1 状态已更新到内存缓存").arg(sn);
}

void RobotManager::onRobotConnectionChanged(const QString &sn, bool connected)
{
    // 连接状态变化处理
    qDebug() << QString("机器人 %1 连接状态变化: %2").arg(sn).arg(connected ? "在线" : "离线");
    
    if (!connected) {
        // 连接断开时的额外处理
        emit robotDisconnected(sn);
    }
}

void RobotManager::initializeHeartbeatDebugLog()
{
    // 创建日志文件目录
    QString logDir = QCoreApplication::applicationDirPath() + "/logs";
    QDir dir(logDir);
    if (!dir.exists()) {
        if (!dir.mkpath(logDir)) {
            qDebug() << "无法创建日志目录: " << logDir;
            m_heartbeatDebugStream = nullptr;
            return;
        }
    }
    
    // 创建心跳调试日志文件名
    QString logFileName = logDir + "/heartbeat_debug_" + QDateTime::currentDateTime().toString("yyyyMMdd_hhmmss") + ".log";
    
    // 创建日志文件对象
    m_heartbeatDebugLog = new QFile(logFileName, this);
    
    // 打开日志文件（写入模式）
    if (m_heartbeatDebugLog->open(QIODevice::WriteOnly | QIODevice::Text)) {
        m_heartbeatDebugStream = new QTextStream(m_heartbeatDebugLog);
        // 写入文件头
        *m_heartbeatDebugStream << "=== Heartbeat Debug Log ===\n";
        *m_heartbeatDebugStream << "Log Start Time: " << QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss.zzz") << "\n";
        *m_heartbeatDebugStream << "========================================\n";
        m_heartbeatDebugStream->flush();
        qDebug() << "心跳调试日志已初始化: " << logFileName;
    } else {
        qDebug() << "无法创建心跳调试日志文件: " << logFileName << " 错误信息: " << m_heartbeatDebugLog->errorString();
        m_heartbeatDebugStream = nullptr;
    }
}

void RobotManager::logHeartbeatDebugInfo(const QString &sn, qint64 timestamp, const QString &info)
{
    // 检查日志流是否有效
    if (!m_heartbeatDebugStream) {
        qDebug() << "心跳调试日志流未初始化或不可用";
        return;
    }
    
    // 检查文件是否仍然打开
    if (!m_heartbeatDebugLog->isOpen()) {
        qDebug() << "心跳调试日志文件已关闭";
        return;
    }
    
    // 写入心跳调试信息
    *m_heartbeatDebugStream << QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss.zzz") 
                           << " | SN: " << sn 
                           << " | Timestamp: " << timestamp
                           << " | Info: " << info << "\n";
    m_heartbeatDebugStream->flush();
    
    // 添加调试信息确认日志已写入
    qDebug() << QString("心跳调试信息已写入日志文件: SN=%1, Timestamp=%2, Info=%3").arg(sn).arg(timestamp).arg(info);
}

RobotClient* RobotManager::findClientBySN(const QString &sn)
{
    return m_tcpServer->findClientBySN(sn);
}

// ==================== 路径跟踪功能实现 ====================

bool RobotManager::startPathTracking(const QString &sn, const QVariantList &pathPoints, int cruiseSpeed)
{
    // 检查机器人是否连接
    RobotClient *client = findClientBySN(sn);
    if (!client || !client->isConnected() || !client->isRegistered()) {
        qDebug() << "路径跟踪失败：机器人未连接或未注册" << sn;
        emit pathTrackingError(sn, "机器人未连接或未注册");
        return false;
    }
    
    // 检查路径点数量
    if (pathPoints.size() < 2) {
        qDebug() << "路径跟踪失败：路径点数量不足" << sn;
        emit pathTrackingError(sn, "路径点数量不足（至少需要2个点）");
        return false;
    }
    
    // 停止之前的跟踪（如果存在）
    stopPathTracking(sn);
    
    // 创建路径跟踪器
    PathTracker *tracker = new PathTracker;
    tracker->currentTargetIndex = 0;
    tracker->cruiseSpeed = qBound(0, cruiseSpeed, 25);
    tracker->arrivalCount = 0;
    tracker->isActive = true;
    tracker->estimatedHeading = 0.0;
    tracker->isNavigatingToStart = false;  // 初始化预导航标志
    tracker->startPointDistance = 0.0;     // 初始化距离
    tracker->lastMotionCommand = 0;        // 默认停止
    
    // 转换路径点
    for (const QVariant &pointVar : pathPoints) {
        QVariantMap point = pointVar.toMap();
        double lon = point["lon"].toDouble();
        double lat = point["lat"].toDouble();
        tracker->pathPoints.append(QGeoCoordinate(lat, lon));
    }
    
    // 获取机器人当前位置
    QVariantMap robotInfo = getRobotInfo(sn);
    QGeoCoordinate robotPos;
    bool hasRobotPos = false;
    
    if (robotInfo.contains("pos_x") && robotInfo.contains("pos_y")) {
        double lon = robotInfo["pos_x"].toDouble();
        double lat = robotInfo["pos_y"].toDouble();
        robotPos = QGeoCoordinate(lat, lon);
        hasRobotPos = true;
        tracker->lastPosition = robotPos;
        tracker->lastPositionTime = QDateTime::currentDateTime();
    } else {
        // 如果没有当前位置，使用第一个路径点作为起始点
        tracker->lastPosition = tracker->pathPoints.first();
        tracker->lastPositionTime = QDateTime::currentDateTime();
        hasRobotPos = false;
    }
    
    // 【修改】按序号顺序导航：从序号1（索引0）开始，依次到达2、3、4...
    // 不再自动寻找最近路径点，严格按照序号顺序
    tracker->isNavigatingToStart = false;
    tracker->currentTargetIndex = 0;  // 从第一个路径点（序号1）开始
    qDebug() << "路径跟踪初始化：从序号1（索引0）开始，按顺序依次到达各路径点";
    
    // 创建控制定时器（50ms周期，20Hz）
    tracker->controlTimer = new QTimer(this);
    tracker->controlTimer->setInterval(50);
    connect(tracker->controlTimer, &QTimer::timeout, this, [this, sn]() {
        onPathTrackingLoop(sn);
    });
    
    // 保存跟踪器
    m_pathTrackers[sn] = tracker;
    
    // 启动定时器
    tracker->controlTimer->start();
    
    qDebug() << "路径跟踪已启动：" << sn 
             << "路径点数：" << tracker->pathPoints.size()
             << "巡航速度：" << tracker->cruiseSpeed
             << "预导航模式：" << (tracker->isNavigatingToStart ? "是" : "否");
    
    return true;
}

void RobotManager::stopPathTracking(const QString &sn)
{
    if (!m_pathTrackers.contains(sn)) {
        return;
    }
    
    PathTracker *tracker = m_pathTrackers[sn];
    
    // 停止定时器
    if (tracker->controlTimer) {
        tracker->controlTimer->stop();
        tracker->controlTimer->deleteLater();
    }
    
    // 发送停止指令
    tracker->lastMotionCommand = 2;  // 强制下发停止指令
    sendTrackingCommand(sn, 0, 0);
    
    // 删除跟踪器
    delete tracker;
    m_pathTrackers.remove(sn);
    
    qDebug() << "路径跟踪已停止：" << sn;
}

bool RobotManager::isPathTracking(const QString &sn) const
{
    return m_pathTrackers.contains(sn) && m_pathTrackers[sn]->isActive;
}

void RobotManager::updateRobotPositionForTracking(const QString &sn, double lon, double lat)
{
    if (!m_pathTrackers.contains(sn)) {
        return;
    }
    
    PathTracker *tracker = m_pathTrackers[sn];
    QGeoCoordinate newPosition(lat, lon);
    
    // 估算朝向（基于位置变化）
    if (tracker->lastPosition.isValid()) {
        double distance = tracker->lastPosition.distanceTo(newPosition);
        if (distance > 0.1) {  // 移动距离大于0.1米才更新朝向
            tracker->estimatedHeading = tracker->lastPosition.azimuthTo(newPosition);
        }
    }
    
    tracker->lastPosition = newPosition;
    tracker->lastPositionTime = QDateTime::currentDateTime();
}

void RobotManager::onPathTrackingLoop(const QString &sn)
{
    if (!m_pathTrackers.contains(sn)) {
        return;
    }
    
    PathTracker *tracker = m_pathTrackers[sn];
    
    // 检查机器人连接状态
    RobotClient *client = findClientBySN(sn);
    if (!client || !client->isConnected() || !client->isRegistered()) {
        qDebug() << "路径跟踪中断：机器人连接断开" << sn;
        emit pathTrackingError(sn, "机器人连接断开");
        stopPathTracking(sn);
        return;
    }
    
    // 检查位置更新超时（超过2秒未更新位置）
    qint64 timeSinceLastUpdate = tracker->lastPositionTime.msecsTo(QDateTime::currentDateTime());
    if (timeSinceLastUpdate > 2000) {
        qDebug() << "路径跟踪中断：位置更新超时" << sn;
        emit pathTrackingError(sn, "位置更新超时");
        stopPathTracking(sn);
        return;
    }
    
    // 【删除】预导航阶段处理已移除，直接按序号顺序导航
    
    // 正常路径跟踪逻辑
    // 检查是否还有目标点
    if (tracker->currentTargetIndex >= tracker->pathPoints.size()) {
        qDebug() << "路径跟踪完成：" << sn;
        emit pathTrackingCompleted(sn);
        stopPathTracking(sn);
        return;
    }
    
    // 检查当前位置是否有效
    if (!tracker->lastPosition.isValid()) {
        qDebug() << "路径跟踪错误：当前位置无效" << sn;
        return;
    }
    
    // 【修改】按序号顺序导航：严格按照currentTargetIndex指向的路径点，不自动切换
    // 获取当前目标点（序号为currentTargetIndex+1）
    QGeoCoordinate targetPos = tracker->pathPoints[tracker->currentTargetIndex];
    
    // 计算距离
    double distanceToTarget = tracker->lastPosition.distanceTo(targetPos);
    
    // 检查是否到达目标点
    const double ARRIVAL_THRESHOLD = 5.0;  // 5米到达阈值
    
    if (distanceToTarget < ARRIVAL_THRESHOLD) {
        tracker->arrivalCount++;
        if (tracker->arrivalCount >= 3) {  // 连续3次都在阈值内，确认到达
            // 切换到下一个目标点
            tracker->currentTargetIndex++;
            tracker->arrivalCount = 0;
            
            qDebug() << "✅ 到达目标点" << (tracker->currentTargetIndex) 
                     << "（序号" << (tracker->currentTargetIndex + 1) << "），切换到目标点" 
                     << (tracker->currentTargetIndex + 1) << "（序号" << (tracker->currentTargetIndex + 2) << "）"
                     << "剩余点数：" << (tracker->pathPoints.size() - tracker->currentTargetIndex - 1);
            
            // 如果还有下一个目标点，继续；否则完成
            if (tracker->currentTargetIndex >= tracker->pathPoints.size()) {
                qDebug() << "路径跟踪完成：" << sn;
                emit pathTrackingCompleted(sn);
                stopPathTracking(sn);
                return;
            }
        }
    } else {
        tracker->arrivalCount = 0;
    }
    
    // 【修改】主要使用相对坐标判断方向，删除所有角度差判断逻辑
    // 使用相对坐标判断方向（以机器人位置为原点建立局部坐标系）
    DirectionInfo dirInfo = calculateDirectionInfo(tracker->lastPosition, targetPos, 0.0, false);
    
    // 根据相对坐标判断结果计算转向值
    int turnValue = 0;
    if (dirInfo.needTurn) {
        if (dirInfo.needUTurn) {
            // 需要掉头，使用最大转向值
            turnValue = (dirInfo.turnAngle > 0) ? 12 : -12;
            qDebug() << "【相对坐标判断】目标点在" << dirInfo.direction << "方向，需要掉头，转向值:" << turnValue;
        } else {
            // 需要转向但不需要掉头，根据转向角度计算转向值
            // 将转向角度映射到[-12, 12]范围
            double normalized = dirInfo.turnAngle / 90.0;  // 归一化到[-1, 1]
            turnValue = static_cast<int>(normalized * 12.0);
            turnValue = qBound(-12, turnValue, 12);
            qDebug() << "【相对坐标判断】目标点在" << dirInfo.direction << "方向，需要转向" 
                     << dirInfo.turnAngle << "度，转向值:" << turnValue;
        }
    } else {
        // 不需要转向，转向值为0
        turnValue = 0;
        qDebug() << "【相对坐标判断】目标点在" << dirInfo.direction << "方向，不需要转向";
    }
    
    // 【优化】计算速度值（根据距离和转向角度调整）
    int speedValue = tracker->cruiseSpeed;
    
    // 如果正在掉头，降低速度
    if (dirInfo.needUTurn) {
        speedValue = qMax(5, tracker->cruiseSpeed / 3);  // 掉头时速度降低到1/3，最小5
        qDebug() << "掉头中，降低速度到:" << speedValue;
    }
    
    // 根据距离调整速度
    if (distanceToTarget < 2.0) {
        speedValue = qMin(speedValue, 10);  // 接近目标时减速
    }
    if (distanceToTarget < 0.5) {
        speedValue = 0;   // 非常接近时停止
    }
    
    // 大角度转向时也减速（非掉头情况）
    if (!dirInfo.needUTurn && qAbs(dirInfo.turnAngle) > 60.0) {
        speedValue = qMin(speedValue, tracker->cruiseSpeed * 2 / 3);
    }
    
    // 发送控制指令
    sendTrackingCommand(sn, turnValue, speedValue);
    
    // 发送进度信号
    emit pathTrackingProgress(sn, tracker->currentTargetIndex, 
                              tracker->pathPoints.size(), distanceToTarget);
}

int RobotManager::calculateTurnValue(double azimuth, double currentHeading, bool isUTurn)
{
    // 计算角度差
    double headingDiff = azimuth - currentHeading;
    
    // 归一化到[-180, 180]
    while (headingDiff > 180.0) headingDiff -= 360.0;
    while (headingDiff < -180.0) headingDiff += 360.0;
    
    // 映射到D121寄存器范围[-12, 12]
    // 使用非线性映射，小角度时更敏感
    double normalized = headingDiff / 180.0;
    int turnValue = 0;
    
    if (isUTurn) {
        // 【修复】掉头处理：使用最大转向值
        if (headingDiff > 0) {
            turnValue = 12;  // 右转掉头
        } else {
            turnValue = -12; // 左转掉头
        }
        qDebug() << "检测到掉头情况，角度差:" << headingDiff << "度，转向值:" << turnValue;
    } else if (qAbs(headingDiff) > 170.0) {
        // 【新增】对于接近180度但不是掉头的情况（反向直行），使用较小的转向值
        // 这样可以避免误判为掉头，同时允许小幅调整方向
        if (headingDiff > 0) {
            turnValue = 2;  // 小幅右转
        } else {
            turnValue = -2; // 小幅左转
        }
        qDebug() << "检测到反向直行情况，角度差:" << headingDiff 
                 << "度，使用小幅转向值:" << turnValue;
    } else if (qAbs(normalized) < 0.1) {
        // 角度差小于18度，微调
        turnValue = static_cast<int>(normalized * 6.0);  // ±6范围内
    } else {
        // 角度差较大，全范围映射
        turnValue = static_cast<int>(normalized * 12.0);
    }
    
    // 限制在[-12, 12]
    turnValue = qBound(-12, turnValue, 12);
    
    return turnValue;
}

void RobotManager::sendTrackingCommand(const QString &sn, int turnValue, int speedValue)
{
    PathTracker *tracker = m_pathTrackers.contains(sn) ? m_pathTrackers.value(sn) : nullptr;
    int motionState = (speedValue > 0) ? 1 : (speedValue < 0 ? -1 : 0);  // 1:前进, -1:后退, 0:停止
    bool motionChanged = true;
    
    if (tracker) {
        if (tracker->lastMotionCommand == motionState) {
            motionChanged = false;
        } else {
            tracker->lastMotionCommand = motionState;
        }
    }
    
    // 1. 先处理转向指令（每次循环都可能需要调整）
    if (turnValue != 0) {
        QVariantMap turnCommand;
        turnCommand["register"] = "D121";
        turnCommand["address"] = 121;
        turnCommand["functionCode"] = 6;
        turnCommand["value"] = turnValue;  // turnValue 已经是正确的符号（正值右转，负值左转）
        sendControlCommand(sn, turnCommand);
    }
    
    // 2. 再处理前进/后退/停止指令：仅在状态发生变化时下发一次
    if (!motionChanged) {
        return;  // 状态未变化，无需重复下发
    }
    
    if (motionState == 0) {
        // 停止
        QVariantList commands = generateRawCommands("stop");
        if (!commands.isEmpty()) {
            QVariantMap request;
            request["rawCommands"] = commands;
            sendControlCommand(sn, request);
        }
        return;
    }
    
    // 前进或后退
    QString direction = (motionState > 0) ? "forward" : "backward";
    QVariantList commands = generateRawCommands(direction);
    if (!commands.isEmpty()) {
        QVariantMap request;
        request["rawCommands"] = commands;
        sendControlCommand(sn, request);
    }
}

// =============== QML控制接口相关实现 ===============

void RobotManager::setQMLControlInterface(QObject *qmlContext)
{
    if (m_controlInterface && qmlContext) {
        m_controlInterface->setQMLContext(qmlContext);
        qDebug() << "QML控制接口已设置上下文";
    }
}

QMLControlInterface* RobotManager::getControlInterface() const
{
    return m_controlInterface;
}

// ==================== 相对坐标方向判断实现 ====================

RobotManager::DirectionInfo RobotManager::calculateDirectionInfo(
    const QGeoCoordinate &robotPos, 
    const QGeoCoordinate &targetPos,
    double robotHeading,
    bool hasRobotHeading)
{
    DirectionInfo info;
    info.needTurn = false;
    info.needUTurn = false;
    info.turnAngle = 0.0;
    info.direction = "未知";
    
    if (!robotPos.isValid() || !targetPos.isValid()) {
        return info;
    }
    
    // 1. 计算相对坐标（米）
    // 以机器人位置为原点建立局部坐标系
    double dx = targetPos.longitude() - robotPos.longitude();
    double dy = targetPos.latitude() - robotPos.latitude();
    
    double lat = robotPos.latitude();
    // 经度方向距离（米）：考虑纬度影响
    // dx_m: 正值表示东（右），负值表示西（左）
    double dx_m = dx * 111320.0 * qCos(qDegreesToRadians(lat));
    // 纬度方向距离（米）
    // dy_m: 正值表示北（前），负值表示南（后）
    double dy_m = dy * 111320.0;
    
    // 2. 计算目标方向角度（以机器人为原点的相对坐标系）
    // 坐标系定义：
    // - 右边（东，x轴正方向）为0度
    // - 前面（北，y轴正方向）为90度
    // - 左边（西，x轴负方向）为180度
    // - 后面（南，y轴负方向）为270度
    // 
    // atan2(x, y) 返回从y轴正方向（前）到点(x,y)的角度：
    // - 前面（x=0, y>0）：0度
    // - 右边（x>0, y=0）：90度
    // - 后面（x=0, y<0）：±180度
    // - 左边（x<0, y=0）：-90度
    // 
    // 需要转换为目标坐标系：右边0度，前面90度，左边180度，后面270度
    // 转换公式：targetAngle = (atan2(x, y) + 90) % 360
    double angle = qAtan2(dx_m, dy_m) * 180.0 / M_PI;  // 从-180到180度
    double targetAngle = angle + 90.0;  // 旋转90度：前面0度->90度，右边90度->0度
    if (targetAngle < 0) targetAngle += 360.0;  // 确保在0-360度范围内
    if (targetAngle >= 360.0) targetAngle -= 360.0;
    
    // 3. 根据角度判断方向和行为
    // 角度判断规则：
    // - 45度到135度：直行
    // - 135度到225度：左转
    // - 225度到315度：掉头
    // - 其余（0-45度，315-360度）：右转
    
    if (targetAngle >= 45.0 && targetAngle < 135.0) {
        // 45度到135度：直行（前方区域）
        info.direction = "前";
        info.needTurn = false;
        info.needUTurn = false;
        info.turnAngle = 0.0;
    } else if (targetAngle >= 135.0 && targetAngle < 225.0) {
        // 135度到225度：左转（左侧区域）
        info.direction = "左";
        info.needTurn = true;
        info.needUTurn = false;
        // 计算左转角度：目标角度 - 90度（前方）
        info.turnAngle = targetAngle - 90.0;
        if (info.turnAngle > 90.0) info.turnAngle = 90.0;  // 限制最大左转90度
    } else if (targetAngle >= 225.0 && targetAngle < 315.0) {
        // 225度到315度：掉头（后方区域）
        info.direction = "后";
        info.needTurn = true;
        info.needUTurn = true;
        // 计算掉头角度：目标角度 - 90度（前方），然后取反
        info.turnAngle = 180.0;  // 掉头需要180度转向
    } else {
        // 0-45度或315-360度：右转（右侧区域）
        info.direction = "右";
        info.needTurn = true;
        info.needUTurn = false;
        // 计算右转角度
        if (targetAngle < 45.0) {
            // 0-45度：右转角度为负值（相对于前方90度）
            info.turnAngle = targetAngle - 90.0;
        } else {
            // 315-360度：右转角度为正值
            info.turnAngle = targetAngle - 90.0;
            if (info.turnAngle > 90.0) info.turnAngle = 90.0;  // 限制最大右转90度
        }
    }
    
    qDebug() << "【相对坐标判断-新规则】机器人位置:" << robotPos.latitude() << "," << robotPos.longitude()
             << "目标位置:" << targetPos.latitude() << "," << targetPos.longitude()
             << "相对坐标(dx,dy):" << dx_m << "," << dy_m << "米"
             << "目标角度:" << targetAngle << "度"
             << "方向:" << info.direction
             << "需要转向:" << info.needTurn
             << "需要掉头:" << info.needUTurn
             << "转向角度:" << info.turnAngle << "度";
    
    return info;
}